﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Vector3 = DDD.Vector3<float>;

namespace DDD {
    /// <summary>
    /// クォータニオン
    /// </summary>
    /// <remarks>
    /// 回転を表すクォータニオンクラスです。
    /// <see cref="X"/>,<see cref="Y"/>,<see cref="Z"/>の3つが虚数部分を表し、
    /// <see cref="W"/>がスカラー要素を表します。
    /// クォータニオンについては例えば「ゲームプログラミングのための3Dグラフィックス数学」を参照してください。
    /// </remarks>
    public struct Quaternion : IExportable<float>, IAnimatable<float>, IEquatable<Quaternion> {

        #region Constructor
        /// <summary>
        /// コンストラクター
        /// </summary>       
        /// <remarks>
        /// 回転軸は正規化されている必要はありません。回転角は度数で<code>[0,360)</code>の範囲で指定します。
        /// 返されたクォータニオンは正規化済みです。
        /// <code>Angle=0,Axis=(0,0,0)</code>はゼロクォータニオンを示す特殊な組み合わせです。
        /// </remarks>
        /// <param name="angle">回転角度(in degree[0,360))</param>
        /// <param name="ax">回転軸のX要素</param>
        /// <param name="ay">回転軸のY要素</param>
        /// <param name="az">回転軸のZ要素</param>
        /// <returns></returns>
        public Quaternion (float angle, float ax, float ay, float az)
            : this () {
            if (angle != 0 && ax == 0 && ay == 0 && az == 0) {
                throw new ArgumentException ("Axis is (0,0,0), but Angle is not 0");
            }

            if (ax == 0 && ay == 0 && az == 0) {
                this.X = 0;
                this.Y = 0;
                this.Z = 0;
                this.W = 1;
                this.IsNormalized = false;
            }
            else {
                var axis = new Vector3 (ax, ay, az).Normalize ();
                var theta = (float)Math.PI * angle / 180.0f;
                this.X = axis.X * (float)Math.Sin (theta / 2.0f);
                this.Y = axis.Y * (float)Math.Sin (theta / 2.0f);
                this.Z = axis.Z * (float)Math.Sin (theta / 2.0f);
                this.W = (float)Math.Cos (theta / 2.0f);
                this.IsNormalized = true;
            }
        }

        /// <summary>
        /// 回転行列(<see cref="Matrix3x3"/>)形式で指定して作成するコンストラクター
        /// </summary>
        /// <remarks>
        /// 返されるクォータニオンは正規化済みです。
        /// </remarks>
        /// <param name="m">回転行列</param>
        public Quaternion (Matrix3x3 m)
            : this () {
            var trace = m[0] + m[4] + m[8];
            if (trace > 0) {
                var s = (float)Math.Sqrt (trace + 1.0f);
                var t = 0.5f / s;
                this[0] = (m[7] - m[5]) * t;
                this[1] = (m[2] - m[6]) * t;
                this[2] = (m[3] - m[1]) * t;
                this[3] = s * 0.5f;
            }
            else {
                var i = 0;
                i = (m[4] > m[0]) ? 1 : i;
                i = (m[8] > m[i * 4]) ? 2 : i;
                var j = (i + 1) % 3;
                var k = (j + 1) % 3;
                var s = (float)Math.Sqrt ((m[i * 4] - (m[j * 4] + m[k * 4])) + 1.0f);
                var t = (s != 0) ? 0.5f / s : s;
                this[i] = s * 0.5f;
                this[j] = (m[j * 3 + i] + m[i * 3 + j]) * t;
                this[k] = (m[k * 3 + i] + m[i * 3 + k]) * t;
                this[3] = (m[k * 3 + j] - m[j * 3 + k]) * t;
            }
            this.IsNormalized = true;
        }
        #endregion

        #region Property
        /// <summary>
        /// X要素
        /// </summary>
        public float X {
            get;
            private set;
        }

        /// <summary>
        /// Y要素
        /// </summary>
        public float Y {
            get;
            private set;
        }

        /// <summary>
        /// Z要素
        /// </summary>
        public float Z {
            get;
            private set;
        }

        /// <summary>
        /// W要素
        /// </summary>
        public float W {
            get;
            private set;
        }

        /// <summary>
        /// コンポーネント数
        /// </summary>
        public int ComponentCount {
            get { return 4; }
        }

        /// <summary>
        /// コンポーネントにアクセスするインデクサー
        /// </summary>
        /// <param name="index">インデックス</param>
        /// <returns></returns>
        public float this[int index] {
            get {
                if (index < 0 || index >= ComponentCount) {
                    throw new IndexOutOfRangeException ("Index is out of range");
                }
                switch (index) {
                    case 0: return X;
                    case 1: return Y;
                    case 2: return Z;
                    case 3: return W;
                    default: return 0;
                }
            }
            set {
                if (index < 0 || index >= ComponentCount) {
                    throw new IndexOutOfRangeException ("Index is out of range");
                }
                switch (index) {
                    case 0: this.X = value; break;
                    case 1: this.Y = value; break;
                    case 2: this.Z = value; break;
                    case 3: this.W = value; break;
                    default: break;
                }
            }
        }

        /// <summary>
        /// クォータニオンの長さ
        /// </summary>
        public float Length {
            get {
                return (float)Math.Sqrt (X * X + Y * Y + Z * Z + W * W);
            }
        }

        /// <summary>
        /// クォータニオンの長さの自乗
        /// </summary>
        public float Length2 {
            get {
                return X * X + Y * Y + Z * Z + W * W;
            }
        }

        /// <summary>
        /// 正規化済みかどうかのフラグ
        /// </summary>
        public bool IsNormalized {
            get;
            private set;
        }

        /// <summary>
        /// このクォータニオンと等価な3x3の回転行列
        /// </summary>
        /// <returns></returns>
        public Matrix3x3 Matrix3x3 {
            get {
                var m00 = 1 - 2 * Y * Y - 2 * Z * Z;
                var m01 = 2 * X * Y - 2 * W * Z;
                var m02 = 2 * X * Z + 2 * W * Y;
                var m10 = 2 * X * Y + 2 * W * Z;
                var m11 = 1 - 2 * X * X - 2 * Z * Z;
                var m12 = 2 * Y * Z - 2 * W * X;
                var m20 = 2 * X * Z - 2 * W * Y;
                var m21 = 2 * Y * Z + 2 * W * X;
                var m22 = 1 - 2 * X * X - 2 * Y * Y;
                return new Matrix3x3 (m00, m01, m02,
                                     m10, m11, m12,
                                     m20, m21, m22);
            }
        }

        /// <summary>
        /// このクォータニオンと等価なAngleAxis形式の回転角度
        /// </summary>
        /// <remarks>
        /// AngleAxis形式で表したこのクォータニオンの回転角度を返します。
        /// 回転角度は度数(in degree[0,180))です。
        /// </remarks>
        public float Angle {
            get {
                return 2 * (float)Math.Acos (W) / (float)Math.PI * 180;
            }
        }

        /// <summary>
        /// AngleAxis形式で表したときの回転軸
        /// </summary>
        /// <remarks>
        /// AngleAxis形式で表したこのクォータニオンの回転軸を返します。
        /// 返される回転軸は必ず正規化されています。
        /// ただし回転角度が0の場合は回転軸は(0,0,0)が返ります。
        /// </remarks>
        public Vector3 Axis {
            get {
                var d = (float)Math.Sqrt (1 - W * W);
                if (d == 0) {
                    return new Vector3 (0, 0, 0);
                }
                return new Vector3 (X, Y, Z).Normalize ();
            }
        }

        #endregion

        #region Method

        /// <summary>
        /// 数値を直接指定してクォータニオンを作成します
        /// </summary>
        /// <remarks>
        /// 引数の<paramref name="normalize"/>フラグをtrueにすると自動で正規化します。
        /// </remarks>
        /// <param name="x">X要素(ベクトル成分i)</param>
        /// <param name="y">Y要素(ベクトル成分j)</param>
        /// <param name="z">Z要素(ベクトル成分k)</param>
        /// <param name="w">W要素(スカラー成分)</param>
        /// <param name="normalize">正規化フラグ</param>
        /// <returns>作成したクォータニオン</returns>
        public static Quaternion Set (float x, float y, float z, float w, bool normalize = false) {
            var q = new Quaternion ();
            q.X = x;
            q.Y = y;
            q.Z = z;
            q.W = w;
            if (normalize) {
                q = q.Normalize ();
            }
            return q;
        }

        /// <summary>
        /// 数値を配列で指定してクォータニオンを作成します
        /// </summary>
        /// <param name="xyzw">XYZW要素</param>
        /// <param name="normalize">正規化フラグ</param>
        /// <returns></returns>
        public static Quaternion Set (float[] xyzw, bool normalize = false) {
            if (xyzw == null) {
                throw new ArgumentNullException ("XYZW is null");
            }
            if (xyzw.Length < 4) {
                throw new ArgumentException ("XYZW is too short");
            }

            return Set (xyzw[0], xyzw[1], xyzw[2], xyzw[3], normalize);
        }

        /// <summary>
        /// 単位クォータニオンを作成します
        /// </summary>
        public static Quaternion Identity {
            get {
                var q = new Quaternion ();
                q.X = 0;
                q.Y = 0;
                q.Z = 0;
                q.W = 1;
                q.IsNormalized = true;
                return q;
            }
        }

        /// <summary>
        /// 逆クォータニオンを作成します
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグは計算前の値と一致します。
        /// </remarks>
        /// <returns>作成された逆クォータニオン</returns>
        public Quaternion Inverse () {
            if (IsNormalized) {
                return Conjugate ();
            }
            return Quaternion.Set (-X / Length2, -Y / Length2, -Z / Length2, W / Length2);
        }

        /// <summary>
        /// 共役のクォータニオンを作成します
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグは計算前の値と一致します。
        /// </remarks>
        /// <returns>作成された共役のクォータニオン</returns>
        public Quaternion Conjugate () {
            var q = Quaternion.Set (-X, -Y, -Z, W);
            if (IsNormalized) {
                q.IsNormalized = true;
            }
            return q;
        }

        /// <summary>
        /// クォータニオンの対数を作成します
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグはfalseです。
        /// </remarks>
        /// <returns>作成された対数クォータニオン</returns>
        public Quaternion Log () {
            var q = new Quaternion ();
            var a = (float)Math.Acos (W);
            if (a > 0) {
                q[0] = a * X / (float)Math.Sin (a);
                q[1] = a * Y / (float)Math.Sin (a);
                q[2] = a * Z / (float)Math.Sin (a);
                q[3] = 0;
            }
            return q;
        }

        /// <summary>
        /// クォータニオンの内積値を計算します
        /// </summary>
        /// <param name="q2">クォータニオン</param>
        /// <returns>内積</returns>
        public float Dot (Quaternion q2) {
            return this[0] * q2[0] + this[1] * q2[1] + this[2] * q2[2] + this[3] * q2[3];
        }

        /// <summary>
        /// クォータニオンのfloatの積
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグはfalseです。
        /// </remarks>
        /// <param name="q">クォータニオン</param>
        /// <param name="f">float値</param>
        /// <returns></returns>
        public static Quaternion operator * (Quaternion q, float f) {
            return Quaternion.Set (q[0] * f, q[1] * f, q[2] * f, q[3] * f);
        }

        /// <summary>
        /// floatとクォータニオンの積
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグはfalseです。
        /// </remarks>
        /// <param name="f">float値</param>
        /// <param name="q">クォータニオン</param>
        /// <returns></returns>
        public static Quaternion operator * (float f, Quaternion q) {
            return q * f;
        }

        /// <summary>
        /// クォータニオンとfloatの割り算
        /// </summary>
        /// <remarks>
        /// ゼロ割はNAN化します。
        /// 計算結果の<see cref="IsNormalized"/>フラグはfalseです。
        /// </remarks>
        /// <param name="q">クォータニオン</param>
        /// <param name="f">float値</param>
        /// <returns></returns>
        public static Quaternion operator / (Quaternion q, float f) {
            return q * (1 / f);
        }

        /// <summary>
        /// クォータニオン同士の足し算
        /// </summary>
        /// <remarks>
        /// 計算結果の<see cref="IsNormalized"/>フラグはfalseです。
        /// </remarks>
        /// <param name="q1">クォータニオン</param>
        /// <param name="q2">クォータニオン</param>
        /// <returns></returns>
        public static Quaternion operator + (Quaternion q1, Quaternion q2) {
            if (q1 == null || q2 == null) {
                throw new ArgumentNullException ("Quaternion is null");
            }
            return Quaternion.Set (q1[0] + q2[0], q1[1] + q2[1], q1[2] + q2[2], q1[3] + q2[3]);
        }

        /// <summary>
        /// クォータニオンを球面補間します
        /// </summary>
        /// <remarks>
        /// 補間元の2つのクォータニオンの<see cref="IsNormalized"/>フラグが両方trueの時に限り
        /// 計算結果のクォータニオンの<see cref="IsNormalized"/>フラグもtrueになります。
        /// それ以外の時はfalseです。
        /// s=0の時クォータニオン1に、s=1の時クォータニオン2に等しくなります。
        /// q1==q2の時sの値に関わらずq1が返ります(フラグもq1に同じ)。
        /// クォータニオンはその性質上2つの「等価」な回転が必ず存在します。
        /// 例えば45(1,0,0)と-45(-1,0,0)は等価な回転と見なされます。
        /// このメソッドが上記どちらのパターンを返すかは不定です。
        /// 補間結果は常にわずかな計算誤差を含みます。
        /// </remarks>
        /// <param name="s">補間位置[0,1]</param>
        /// <param name="q1">クォータニオン1</param>
        /// <param name="q2">クォータニオン2</param>
        /// <returns></returns>
        public static Quaternion Slerp (float s, Quaternion q1, Quaternion q2) {
            if (s < 0 || s > 1) {
                throw new ArgumentException ("S is invalid");
            }
            if (q1 == null || q2 == null) {
                throw new ArgumentException ("Quaternion is null");
            }

            var a = (float)Math.Acos (q1.Dot (q2));
            if (a == 0) {
                return q1;
            }
            var q = (q1 * (float)Math.Sin ((1 - s) * a) + q2 * (float)Math.Sin (s * a)) / (float)Math.Sin (a);
            if (q1.IsNormalized && q2.IsNormalized) {
                q.IsNormalized = true;
            }
            return q;
        }

        /// <summary>
        /// 正規化して長さが１のクォータニオンを作成します
        /// </summary>
        /// <remarks>
        /// 長さがゼロのクォータニオンを正規化するとNAN化します。
        /// </remarks>
        /// <returns>長さが1のクォータニオン</returns>
        public Quaternion Normalize () {
            if (IsNormalized) {
                return this;
            }
            var q = new Quaternion ();
            q.X = X / Length;
            q.Y = Y / Length;
            q.Z = Z / Length;
            q.W = W / Length;
            q.IsNormalized = true;
            return q;
        }


        /// <summary>
        /// このクォータニオンを使ってベクトル<dic>vc</dic>を回転します。<code>vc' = qvq'</code>.
        /// </summary>
        /// <remarks>
        /// 計算式は<code>vc' = qvq'</code>. (<dic>q'</dic>は共役のクォータニオンを示す記号）
        /// </remarks>
        /// <param name="v">元のベクトル</param>
        /// <returns>回転後のベクトル</returns>
        public Vector3 Apply (Vector3 v) {
            var a = new Vector3 (X, Y, Z);
            return (W * W - a.Length2) * v + 2 * W * Vector3.Cross (a, v) + 2 * Vector3.Dot (a, v) * a;
        }

        /// <summary>
        /// クォータニオンの積
        /// </summary>
        /// <remarks>
        /// クォータニオンの積を計算します。<code>q' = q1q2</code>.
        /// 定義から<code>q' = (q1q2)vc(q1q2)' = q1(q2vq2')q1'</code>.
        /// 従って新しいクォータニオンはまずクォータニオンq2で回転し
        /// 次にクォータニオンq1で回転するのと等価です。
        /// q1,q2の<see cref="IsNormalized"/>フラグが両方ともtrueの時に限り
        /// 計算結果のクォータニオンの<see cref="IsNormalized"/>フラグもtrueになります。
        /// それ以外の時はfalseです。
        /// </remarks>
        /// <param name="q1">クォータニオン1</param>
        /// <param name="q2">クォータニオン2</param>
        /// <returns>新しいクォータニオン</returns>
        public static Quaternion operator * (Quaternion q1, Quaternion q2) {
            var x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
            var y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
            var z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
            var w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
            var q = Quaternion.Set (x, y, z, w);
            if (q1.IsNormalized && q2.IsNormalized) {
                q.IsNormalized = true;
            }
            return q;
        }

        /// <summary>
        /// 全要素をfloatの配列に変換します
        /// </summary>
        /// <param name="q">変換元のベクトル</param>
        /// <returns>変換後のfloatの配列</returns>
        public static explicit operator float[] (Quaternion q) {
            return new float[] { q[0], q[1], q[2], q[3] };
        }

        /// <summary>
        /// 同値性を比較します
        /// </summary>
        /// <remarks>
        /// 同値性を比較する汎用の比較関数です。
        /// 比較対象はX,Y,Z,Wのみで行われIsNormalizedフラグは考慮されません。
        /// コンポーネント要素が厳密に一致しなくても、
        /// 全ての要素で差が<see cref="GlobalSettings.Tolerance"/>以下であれば等しいと見なされます。
        /// (注意)クォータニオンはその性質上値は異なるが「等価」なものが存在する。
        /// 例えば45,(1,0,0)と-45(-1,0,0)は値は異なるが等価な回転と見なせる。
        /// このメソッドでは要素毎の値を調べるだけで上記の「等価」な回転は考慮していない。
        /// これに関してどう取り扱うかは現在悩み中。
        /// </remarks>
        /// <param name="obj">右辺のオブジェクト</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public override bool Equals (object obj) {
            if (object.ReferenceEquals (obj, null)) {
                return false;
            }
            if (object.ReferenceEquals (obj, this)) {
                return true;
            }
            var q = obj as Quaternion?;
            if (q == null) {
                return false;
            }

            return this.Equals (q.Value);
        }

        /// <summary>
        /// タイプセーフな同値性を比較します
        /// </summary>
        /// <remarks>
        /// ２つのクォータニオンの同値性を判定します。
        /// <see cref="GlobalSettings.Tolerance"/>以内の誤差を許容します。
        /// </remarks>
        /// <param name="q">クォータニオン</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public bool Equals (Quaternion q) {
            return (Math.Abs (X - q.X) < GlobalSettings.Tolerance) &&
                    (Math.Abs (Y - q.Y) < GlobalSettings.Tolerance) &&
                     (Math.Abs (Z - q.Z) < GlobalSettings.Tolerance) &&
                     (Math.Abs (W - q.W) < GlobalSettings.Tolerance);
        }

        /// <summary>
        /// ２つのクォータニオンの等号による同値性を比較します
        /// </summary>
        /// <param name="left">左項</param>
        /// <param name="right">右項</param>
        /// <returns>等しいときtrue, そうでないときfalse</returns>
        public static bool operator == (Quaternion left, Quaternion right) {
            return left.Equals (right);
        }

        /// <summary>
        /// 等号による同値性を比較します
        /// </summary>
        /// <param name="left">左項</param>
        /// <param name="right">右項</param>
        /// <returns>等しくないときtrue, そうでないときfalse</returns>
        public static bool operator != (Quaternion left, Quaternion right) {
            return !(left == right);
        }

        /// <summary>
        /// 一意のハッシュ値を取得します
        /// </summary>
        /// <remarks>
        /// このメソッドは同値性の比較メソッドと異なり
        /// <see cref="GlobalSettings.Tolerance"/>の影響を受けません。
        /// </remarks>
        /// <returns></returns>
        public override int GetHashCode () {
            return base.GetHashCode ();
        }

        /// <summary>
        /// 現在のオブジェクトの状態を表す文字列を返します。
        /// </summary>
        /// <returns>現在のオブジェクトを説明する文字列</returns>
        public override string ToString () {
            return string.Format ("Quaternion : X={0}, Y={1}, Z={2}, W={3}", X, Y, Z, W);
        }
        #endregion

    }
}
